ROS中的路径规划算法dijkstra和A*

ROS Navigation包里面的GlobalPlanner自带是提供了两种全局路径规划的方法，dijkstra和A*
dijkstra 大家都知道了，A*在之前的一篇博客里面有介绍
A*
评估函数 为 f(n) = g(n) + h(n)
g(n) 为起点到已经搜寻过的节点的代价值
h(n) 为当前节点到目标节点(终点)的值
在这里我们可以看到A* 考虑了已经遍历过的点和未遍历过的点两个部分综合起来考虑整体来评估代价值
特别的，当f(n) = g(n)的时候为Dijkstra算法

这里附上ROS的介绍效果图

这里还有一个值得注意的点，在ROS自带Navigation的实现里面是没有提供类似D*这样的动态方法的(当然网上有很多大神有实现D_starGlobalPlanner)，而是用了定时规划路径(也就是在行走过程中也是在不断定时规划路径的，这个feature是可以去掉的，特别是当你的运算负载很高，处理器又有限的情况下)，和重新规划(当找不到路径，也就是走着走着新扫描到未知区域的障碍或者动态增加的障碍)两种办法。在这里加上了定时规划和重新规划之后的A*和D*几乎是一模一样的

---

这里讲解一个例子

每个节点内部的灰色部分是它的h(x)，注意这里的h(x)并不是由它到终点的路径来计算的，可以看作是随意定义的吧，实际情况上这个h(x)的选取本来就是各有不同的。只是在栅格地图中我们用的是欧氏距离

1.根据图中来初始化每个节点的f(x)函数的值
例如
f(B) = 1 + 2 = 3 （Start到B的距离 加上 B的h(x)值也就是中心灰色的值）
f(A) = 1 + 3 = 4
f(C) = 1 + 3 = 4

可得

显然，目前还没有找到Start到Goal的路径，所以进行扩充(expand)

2.从Start的邻近节点开始扩充

上面图中的这个扩充应该不难理解。
显然我们现在得到了一个路径
Start -> A -> E -> Goal 路径的代价值为1+1+3 = 5

3.但是目前我们不确定有没有更短的路径，所以这一步需要先删除代价值大于5的路径，然后再进行扩充

这里D，I，F，G就被移除了，不对它们进行扩充了

再继续进行扩充，现在还可以进行扩充的就是节点C了

这里得到了一条Start -> C -> K -> Goal 的路径，代价值为4

所以得到的最终路径为Start -> C -> K -> Goal